#include "../motion9_dll.h"
#include "stdlib.h"
#include "cubic.h"


//************************************
// Method:    cbCalcFactor
// FullName:  cbCalcFactor
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_CUBIC_PARAM * para
// Parameter: STRUCT_CUBIC_FACTOR factor
// Parameter: float t
//************************************
int cbComputeFactor(STRUCT_CUBIC_PARAM *para, STRUCT_CUBIC_FACTOR *factor, float t)
{
	float tt = t*t;
	float ttt = tt*t;
	factor->a0 = para->posit[0];
	factor->a1 = para->vel[0];
	factor->a2 = (-3 * (para->posit[0] - para->posit[1])
		- (2 * para->vel[0] + para->vel[1])*t) / tt;
	factor->a3 = (2 * (para->posit[0] - para->posit[1])
		+ (para->vel[0] + para->vel[1])*t) / ttt;
	return 0;
}



//************************************
// Method:    cbComputePVA
// FullName:  cbComputePVA
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_CUBIC_FACTOR * factor
// Parameter: float t
// Parameter: double * posit
// Parameter: float * vel
// Parameter: float * acc
//************************************
int cbComputePVA(STRUCT_CUBIC_FACTOR *factor,float t,double *posit,float *vel, float *acc)
{
	float tt = t*t;
	float ttt = tt*t;
	if (posit != NULL){
		*posit = factor->a3*ttt + factor->a2*tt + factor->a1*t + factor->a0;
	}
	if (vel != NULL){
		*vel = 3 * factor->a3*tt + 2 * factor->a2*t + factor->a1;
	}
	if (acc != NULL){
		*acc = 6 * factor->a3*t + 2 * factor->a2;
	}
	return 0;
}
